/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef LASERSAFETYDETECT_H
#define LASERSAFETYDETECT_H
#include <vector>
#include "GeneralDefine.h"
#include <qstring.h>
#include <qmutex.h>
#include "gSoapFoundation/soapStub.h"
#include "globalDefine.h"

class RobotIo;
class MotionMessage;


/**
 * @brief The LaserSafetyDetect class 激光壁障分析
 */
class LaserSafetyDetect
{
public:
    LaserSafetyDetect(int robotIdIn,MotionMessage *motionMessageIn, QString safeFileIn,
                      QString carBorderFileIn, RobotIo *ioControllerIn);

public:
    int controlTypeSwitch(int controlTypeIn);
    int setVehicleDirection(bool isPositiveIn);
    int setMotionStatus(E_MOTION_ID statusIn);
//    int setUseSheftRegion(bool isUseIn);//not used
    int setCurrentVehicleBorderId(int idIn);//更新等效边框．
    int getCurrentVehicleBorderId();
    int getAgvBorderInfo(struct VehicleBorderInfo &infoOut);
    int setAgvBorderRectInfo(struct BorderRectInfo rectInfoIn,int &returnFlag);//立刻生效
    int getAgvSheftInfo(int sheftIndex,struct BorderRectInfo &infoOut);
    int setAgvSheftRectInfo(int sheftIndex,struct BorderRectInfo rectInfoIn,int &returnFlag);//立刻生效
    //立刻生效.不保存进入文件
    int setVehicleBorderOffset(double front_offsetIn,double back_offsetIn,double left_offsetIn,double right_offsetIn);


    E_BARRIER_RUN_MODE getBarrierRunMode();
    int setBarrierRunMode(E_BARRIER_RUN_MODE modeIn);
    int getCollisionParameter(double &barrierSlowDownDistance_XOut, double &barrierSlowDownRatioOut,
                              double &barrierStopDistanceXOut, double &barrierStopDistanceYOut);
    int getSafeCollisionInfo(int sensorType, CollisionPara &infoOut);
    int setSafeCollisionInfo(int sensorType, CollisionPara infoIn,int &returnFlag);//立刻生效

    int getTimeConsumed(double &minConsumedOut,double &currentConsumedOut,double &maxConsumedOut);
    int getCarBorder(std::vector<VehiclePosition> &carBorderOut);
    int getCarBorderOffset(std::vector<VehiclePosition> &carBorderOffsetOut);//获取有效边框
    E_BARRIER_TETECT_MODE getDetectMode();
    int getPointCloudSafeStatus(std::vector<std::string > &sensorNameOut,std::vector<double> &sensorSafeRatioOut,
                                bool &isCheckEnableOut);
    int setPointCloudCheckEnable(bool isOn);
    double getSafetyRatioOut();
    double getSafetySlowTime();


    /**
     * @brief setVehicleVelocity
     * @param vel_x 相对车体前进方向　ｍ/s
     * @param vel_y 相对车体侧方向　ｍ/s
     * @param vel_r　rad/s
     * @return
     */
    int setVehicleVelocity(VehicleVelocity velIn);

    int setPointCloudData(const QVector<PointCloudData_3D> &pointDataIn);




private:
    int writeAgvBorderRectInfo(struct BorderRectInfo rectInfoIn);
    int writeAgvSheftRectInfo(int sheftIndex,struct BorderRectInfo rectInfoIn);

    int calculateIolaserBarrierLogic(E_BARRIER_TETECT_MODE detectModeIn, const IoLaserPara &ioLaserIn, double &safeRatioOut);
    int ioLaserOutputLogic(const IoLaserPara &ioLaserIn, double &safeRatioOut);
    int calculateDetectModeByVel(VehicleVelocity velIn);
    int calculateAdjustPara(VehicleVelocity velIn);
    int calculateDetectModeByMotionStatus(VehicleVelocity velIn,E_MOTION_ID motionStatusIn,bool isMovePositiveIn);
    int updateDetectRegion(VehicleVelocity vehicleVelocityIn);
    int calculateCarOffsetRegion();
    int calculateCarOffsetRegion(std::vector<double> border_x,std::vector<double> border_y);

    int calculateStopRegion();
    int calculateStopRegion(E_BARRIER_TETECT_MODE detectModeIn);
    int calculateStopRegion_para(double frontOffset,double backOffset,double leftOffset,double rightOffset);

    int calculateCeaseRegion_front();//找到一个高速容易减速的ｂｕｇ,边界计算错误
    int calculateCeaseRegion_front_para(double barrierSlowDownDistanceIn,double pieExtendedDistanceIn
                                        ,double besideSlowDownDistanceIn);
    int calculateCeaseRegion_back();
    int calculateCeaseRegion_back_para(double barrierSlowDownDistanceIn,double pieExtendedDistanceIn
                                       ,double besideSlowDownDistanceIn);
    int calculateCeaseRegion_left();
    int calculateCeaseRegion_left_para(double barrierSlowDownDistanceIn, double pieExtendedDistanceIn, double besideSlowDownDistanceIn);
    int calculateCeaseRegion_right();
    int calculateCeaseRegion_right_para(double barrierSlowDownDistanceIn,double pieExtendedDistanceIn
                                        ,double besideSlowDownDistanceIn);
    int calculateCeaseRegion_rotation(double distanceX, double distanceY);
    int calculateCeaseRegion_rotation();

    int calculateCeaseRegion_front_left();
    int calculateCeaseRegion_front_left_para(double distance);
    int calculateCeaseRegion_front_right();
    int calculateCeaseRegion_front_right_para(double distance);
    int calculateCeaseRegion_back_left();
    int calculateCeaseRegion_back_left_para(double distance);
    int calculateCeaseRegion_back_right();
    int calculateCeaseRegion_back_right_para(double distance);


    int calculatePointCloudData(const PointCloudData_3D &pointDataIn, double &safeRatioOut);
    int readCollisionConfigFile(QString configFilePathIn, SafeCollisionInfo &collisionInfoOut,
                                bool &isUseNewControlMethodOut, QString &readComent);
    int writeCollisionConfigFile(QString configFilePathIn,SafeCollisionInfo collisionInfoIn);
    int initialCollisionParameter(SafeCollisionInfo collisionInfoIn);
    int readCarBorderFile(QString configFilePathIn);
    int saveVehicleBorderFile_offset(QString filePathIn);

//    bool isInSheftRegionStatus(double testx, double testy);
//    bool isInSheftXRegion(double testx, double testy);
//    bool isInSheftYRegion(double testx, double testy);
    bool isInRegion(double testx, double testy, std::vector<double>border_x, std::vector<double>border_y);

    int updateSlowDownDistance(double composedVelIn,std::vector<SlowAdjust> adjustParaIn);

    void addMsg(int messageLevel, std::string componentName, std::string messageType
                , int messageCode, int robotIdIn, double parameter1=0, double parameter2=0,
                int parameter3=0, int parameter4=0, QString comment="");


private:
    E_BARRIER_RUN_MODE barrierRunMode;
    double detect_min_z;
    double detect_max_z;
    double safeRatio;
    double slowTime;//减速耗费时间
    double barrierSlowDownRatio;

    //common
    double barrierSlowDownDistance_x;//
    double barrierSlowDownDistance_y;//
    double barrierStopDistance_x;
    double barrierStopDistance_y;
    double pieExtendedDistance;
    std::vector<SlowAdjust> adjustPara;

    //vmark
    double barrierStopDistance_x_vmark;//modify
    double barrierStopDistance_y_vmark;//modify
    double barrierSlowDownDistance_x_vmark;//new
    double barrierSlowDownDistance_y_vmark;//new
    double pieExtendedDistance_vmark;//new
    std::vector<SlowAdjust> adjustPara_vmark;

    //narrowA
    double barrierStopDistance_x_narrowA;
    double barrierStopDistance_y_narrowA;
    double barrierSlowDownDistance_x_narrowA;
    double barrierSlowDownDistance_y_narrowA;
    double pieExtendedDistance_narrowA;
    std::vector<SlowAdjust> adjustPara_narrowA;

    //narrowB
    double barrierStopDistance_x_narrowB;
    double barrierStopDistance_y_narrowB;
    double barrierSlowDownDistance_x_narrowB;
    double barrierSlowDownDistance_y_narrowB;
    double pieExtendedDistance_narrowB;
    std::vector<SlowAdjust> adjustPara_narrowB;




    double front_offset;//对普通和货架都有作用
    double back_offset;
    double left_offset;
    double right_offset;

    int robotId;
    MotionMessage *motionMessage;
    QString carBorderFile;

    std::vector<double> origin_car_border_x;//从左上、右上、右下、左下的顺序
    std::vector<double> origin_car_border_y;

    std::vector<double> validBorder_x;//common sheft　offset都会作用在这里．
    std::vector<double> validBorder_y;

    std::vector<double> zone_stop_x;
    std::vector<double> zone_stop_y;

    std::vector<double> zone_slow_x;
    std::vector<double> zone_slow_y;

    std::vector<double> sheft_borderA_x;//料车区域Ｘ方向
    std::vector<double> sheft_borderA_y;

    std::vector<double> sheft_borderB_x;//料车区域Y方向
    std::vector<double> sheft_borderB_y;

    VehicleVelocity vehicleVel;

    bool isDetectEnable;

    std::vector<std::string > sensorName;
    std::vector<double> sensorSafeRatio;

    /**
     * @brief mutex                 mutex同步锁
     */
    QMutex mutex;

    E_BARRIER_TETECT_MODE detectMode;

    double minConsumed;
    double currentConsumed;
    double maxConsumed;

//    bool isUseSheft;

    QString collisionConfigFile;
    bool isUseIoLaser;
    std::vector<IoLaserPara> ioLaserConfigList;
    RobotIo *ioController;
    int vehicleBorderId;//0普通，１料架a，２料架b
    SafeCollisionInfo collisionInfoAll;
    bool isUseNewControlMethod;
    E_MOTION_ID motionStatus;
    bool isMovePositive;
    EN_CONTROL_TYPE controlType;
};

#endif // LASERSAFETYDETECT_H
